#include "ModelAttributesCalculator.h"
#include "ModelFormation.h"

QVector3D ModelAttributesCalculator::calModelCentroid(const QVector<Triangle>& stl_model_triangles)
{
    float totalArea = 0.0f; // 总面积
    QVector3D weightedCentroid(0.0f, 0.0f, 0.0f); // 加权质心

    // 遍历所有三角形面片
    for (const Triangle& mesh : stl_model_triangles)
    {
        // 获取三角形的三个顶点
        QVector3D v1 = mesh.v1;
        QVector3D v2 = mesh.v2;
        QVector3D v3 = mesh.v3;

        // 计算三角形的质心（三个顶点的平均值）
        QVector3D centroid = (v1 + v2 + v3) / 3.0f;

        // 计算三角形的面积（通过叉积计算）
        QVector3D edge1 = v2 - v1;
        QVector3D edge2 = v3 - v1;
        QVector3D crossProduct = QVector3D::crossProduct(edge1, edge2);
        float area = 0.5f * crossProduct.length(); // 三角形面积

        // 累加加权质心
        weightedCentroid += area * centroid;
        totalArea += area;
    }

    // 计算最终质心
    if (totalArea > 0.0f)
        return weightedCentroid / totalArea;
    else
        return QVector3D(0.0f, 0.0f, 0.0f); // 如果面积为 0，返回默认值
}

// 手动实现 3x3 矩阵的特征值分解（雅可比迭代法）
void jacobiEigenDecomposition(float matrix[3][3], float eigenvalues[3], float eigenvectors[3][3])
{
    const int maxIterations = 100;
    const float epsilon = 1e-6f;

    // 初始化特征向量矩阵为单位矩阵
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            eigenvectors[i][j] = (i == j) ? 1.0f : 0.0f;

    // 雅可比迭代
    for (int iter = 0; iter < maxIterations; ++iter)
    {
        // 找到最大的非对角元素
        int p = 0, q = 1;
        float maxVal = fabs(matrix[0][1]);
        for (int i = 0; i < 3; ++i)
            for (int j = i + 1; j < 3; ++j)
                if (fabs(matrix[i][j]) > maxVal)
                {
                    maxVal = fabs(matrix[i][j]);
                    p = i;
                    q = j;
                }

        // 如果最大非对角元素已经很小，停止迭代
        if (maxVal < epsilon) break;

        // 计算旋转角度
        float theta = 0.5f * atan2(2 * matrix[p][q], matrix[q][q] - matrix[p][p]);
        float c = cos(theta);
        float s = sin(theta);

        // 更新矩阵
        float newMatrix[3][3];
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                if (i == p && j == p)
                    newMatrix[i][j] = c * c * matrix[p][p] - 2 * s * c * matrix[p][q] + s * s * matrix[q][q];
                else if (i == q && j == q)
                    newMatrix[i][j] = s * s * matrix[p][p] + 2 * s * c * matrix[p][q] + c * c * matrix[q][q];
                else if ((i == p && j == q) || (i == q && j == p))
                    newMatrix[i][j] = (c * c - s * s) * matrix[p][q] + s * c * (matrix[p][p] - matrix[q][q]);
                else
                    newMatrix[i][j] = matrix[i][j];

        // 更新特征向量矩阵
        float newEigenvectors[3][3];
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
                newEigenvectors[i][j] = eigenvectors[i][j];

        for (int i = 0; i < 3; ++i)
        {
            newEigenvectors[i][p] = c * eigenvectors[i][p] - s * eigenvectors[i][q];
            newEigenvectors[i][q] = s * eigenvectors[i][p] + c * eigenvectors[i][q];
        }

        // 更新矩阵和特征向量
        for (int i = 0; i < 3; ++i)
            for (int j = 0; j < 3; ++j)
            {
                matrix[i][j] = newMatrix[i][j];
                eigenvectors[i][j] = newEigenvectors[i][j];
            }
    }

    // 提取特征值
    for (int i = 0; i < 3; ++i)
        eigenvalues[i] = matrix[i][i];
}

QVector3D ModelAttributesCalculator::calModelInitPhaseNorm(const QVector<Triangle>& stl_model_triangles) {
    // 提取所有顶点
    QVector<QVector3D> vertices;
    for (const Triangle& mesh : stl_model_triangles)
    {
        vertices.append(mesh.v1);
        vertices.append(mesh.v2);
        vertices.append(mesh.v3);
    }

    // 计算顶点集的均值（中心点）
    QVector3D mean(0.0f, 0.0f, 0.0f);
    for (const QVector3D& vertex : vertices)
        mean += vertex;
    mean /= vertices.size();

    // 构建协方差矩阵
    float covariance[3][3] = {0};
    for (const QVector3D& vertex : vertices)
    {
        QVector3D diff = vertex - mean;
        covariance[0][0] += diff.x() * diff.x();
        covariance[0][1] += diff.x() * diff.y();
        covariance[0][2] += diff.x() * diff.z();
        covariance[1][0] += diff.y() * diff.x();
        covariance[1][1] += diff.y() * diff.y();
        covariance[1][2] += diff.y() * diff.z();
        covariance[2][0] += diff.z() * diff.x();
        covariance[2][1] += diff.z() * diff.y();
        covariance[2][2] += diff.z() * diff.z();
    }
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            covariance[i][j] /= vertices.size();

    // 手动计算特征值和特征向量
    float eigenvalues[3];
    float eigenvectors[3][3];
    jacobiEigenDecomposition(covariance, eigenvalues, eigenvectors);

    // 找到最大特征值对应的特征向量
    int maxIndex = 0;
    float maxEigenvalue = eigenvalues[0];
    for (int i = 1; i < 3; ++i)
        if (eigenvalues[i] > maxEigenvalue)
        {
            maxEigenvalue = eigenvalues[i];
            maxIndex = i;
        }

    // 返回主方向
    return QVector3D(eigenvectors[0][maxIndex], eigenvectors[1][maxIndex], eigenvectors[2][maxIndex]);
}

std::pair<QVector<Triangle>, QVector<Edge> > job1(const QVector<Triangle> &triangles, const QVector<Edge> &edges, const QVector3D &centroid, const QVector3D &planeNormal, float radius)
{
    // 膨胀三角形
    QVector<Triangle> inflatedTriangles(triangles.size());
    std::for_each(std::execution::par, inflatedTriangles.begin(), inflatedTriangles.end(),
                  [&](auto& inflatedTriangle) {
                      int i = &inflatedTriangle - &inflatedTriangles[0];
                      const Triangle& original = triangles[i];

                      // 应用投影变换到每个顶点
                      inflatedTriangle.v1 = projectVertexToCylinder(original.v1, original.v1_color, centroid, planeNormal, radius, original.normal);
                      inflatedTriangle.v2 = projectVertexToCylinder(original.v2, original.v2_color, centroid, planeNormal, radius, original.normal);
                      inflatedTriangle.v3 = projectVertexToCylinder(original.v3, original.v3_color, centroid, planeNormal, radius, original.normal);

                      // 保持颜色不变
                      inflatedTriangle.v1_color = original.v1_color;
                      inflatedTriangle.v2_color = original.v2_color;
                      inflatedTriangle.v3_color = original.v3_color;

                      // 保持法向量不变
                      inflatedTriangle.normal = original.normal;

                      inflatedTriangle.info = original.info;
                  }
                  );

    // 膨胀边（基于膨胀后的三角形重新生成）
    std::vector<Edge> inflatedEdgesVector(3 * inflatedTriangles.size());
    std::for_each(std::execution::par, inflatedTriangles.begin(), inflatedTriangles.end(),
                  [&](const Triangle& t){
                      int i = &t - &inflatedTriangles[0];

                      // 为每个顶点创建完整的Vertex结构
                      Vertex v1, v2, v3;
                      v1.pos = t.v1; v1.norm = t.normal; v1.color = t.v1_color;
                      v2.pos = t.v2; v2.norm = t.normal; v2.color = t.v2_color;
                      v3.pos = t.v3; v3.norm = t.normal; v3.color = t.v3_color;

                      // 三条边
                      Edge e1{v1, v2, t.normal};
                      Edge e2{v2, v3, t.normal};
                      Edge e3{v3, v1, t.normal};

                      inflatedEdgesVector[3 * i] = e1;
                      inflatedEdgesVector[3 * i + 1] = e2;
                      inflatedEdgesVector[3 * i + 2] = e3;
                  }
                  );

    // 去重
    std::sort(inflatedEdgesVector.begin(), inflatedEdgesVector.end());
    auto last = std::unique(inflatedEdgesVector.begin(), inflatedEdgesVector.end());
    inflatedEdgesVector.erase(last, inflatedEdgesVector.end());

    QVector<Edge> inflatedEdges(inflatedEdgesVector.begin(), inflatedEdgesVector.end());

    return {inflatedTriangles, inflatedEdges};
}

void job2(QVector<QVector<QVector<Vertex>>> &contours, const QVector3D &centroid, const QVector3D &planeNormal, float radius)
{
    // // 存储每层收缩后的平均高度
    // QVector<float> layer_heights(contours.size());

    // 处理每一层
    for (int layer_idx = 0; layer_idx < contours.size(); ++layer_idx) {
        auto& sliceContours = contours[layer_idx];
        float total_height = 0.0f;
        int point_count = 0;

        // 处理该层的所有轮廓和顶点
        for (auto& contour : sliceContours) {
            for (auto& vertex : contour) {

                // 执行收缩操作
                float k = job0(vertex);

                // 计算径向方向向量
                QVector3D relativePosa = vertex.sptr_endPointa->pos - QVector3D(centroid.x(), vertex.sptr_endPointa->pos.y(), centroid.z());
                QVector3D relativePosb = vertex.sptr_endPointb->pos - QVector3D(centroid.x(), vertex.sptr_endPointb->pos.y(), centroid.z());

                // 计算其端点的 t 和 gt 权重
                float ta = qAbs(QVector3D::dotProduct(vertex.sptr_endPointa->norm.normalized(), planeNormal.normalized()));
                float tb = qAbs(QVector3D::dotProduct(vertex.sptr_endPointb->norm.normalized(), planeNormal.normalized()));
                float gta = QVector4D::dotProduct(vertex.sptr_endPointa->color, {0.299f, 0.587f, 0.114f, 0.0f});
                float gtb = QVector4D::dotProduct(vertex.sptr_endPointb->color, {0.299f, 0.587f, 0.114f, 0.0f});

                // 计算端点的模型源点
                QVector3D Oa = vertex.sptr_endPointa->pos - relativePosa.normalized() * (exp(ta + gta) - 1) * radius;
                QVector3D Ob = vertex.sptr_endPointb->pos - relativePosb.normalized() * (exp(tb + gtb) - 1) * radius;

                // 线性降采样交点到OaOb向量上
                vertex.pos = Oa + k * (Ob - Oa);

                // 计算该顶点在平面法向量方向上的高度
                float height = QVector3D::dotProduct(vertex.pos, planeNormal);
                total_height += height;
                point_count++;

                // 重置端点
                vertex.sptr_endPointa.reset();
                vertex.sptr_endPointb.reset();
            }
        }
    }
}

// void job2(QVector<QVector<QVector<Vertex> > > &contours, QVector<float>& slice_intervals, const QVector3D &centroid, const QVector3D &planeNormal, float radius)
// {
//     for (auto& sliceContours : contours) {
//         for (auto& contour : sliceContours) {
//             for (auto& vertex : contour) {

//                 // if(vertex.sptr_endPointa || vertex.sptr_endPointb)
//                 // {
//                 //     QVector3D relativePos = vertex.pos - QVector3D(centroid.x(), vertex.pos.y(), centroid.z());
//                 //     float t = qAbs(QVector3D::dotProduct(vertex.norm.normalized(), planeNormal.normalized()));
//                 //     float gt = QVector4D::dotProduct(vertex.color, {0.299f, 0.587f, 0.114f, 0.0f});
//                 //     vertex.pos = vertex.pos - relativePos.normalized() * (exp(t + gt) - 1) * radius;
//                 //     vertex.sptr_endPointa.reset();
//                 //     vertex.sptr_endPointb.reset();
//                 // }
//                 // else
//                 {
//                     float k = job0(vertex);

//                     // 计算径向方向向量
//                     QVector3D relativePosa = vertex.sptr_endPointa->pos - QVector3D(centroid.x(), vertex.sptr_endPointa->pos.y(), centroid.z());
//                     QVector3D relativePosb = vertex.sptr_endPointb->pos - QVector3D(centroid.x(), vertex.sptr_endPointb->pos.y(), centroid.z());

//                     // 计算其端点的 t 和 gt 权重
//                     float ta = qAbs(QVector3D::dotProduct(vertex.sptr_endPointa->norm.normalized(), planeNormal.normalized()));
//                     float tb = qAbs(QVector3D::dotProduct(vertex.sptr_endPointb->norm.normalized(), planeNormal.normalized()));
//                     float gta = QVector4D::dotProduct(vertex.sptr_endPointa->color, {0.299f, 0.587f, 0.114f, 0.0f});
//                     float gtb = QVector4D::dotProduct(vertex.sptr_endPointb->color, {0.299f, 0.587f, 0.114f, 0.0f});

//                     // 计算端点的模型源点
//                     QVector3D Oa = vertex.sptr_endPointa->pos - relativePosa.normalized() * (exp(ta + gta) - 1) * radius;
//                     QVector3D Ob = vertex.sptr_endPointb->pos - relativePosb.normalized() * (exp(tb + gtb) - 1) * radius;

//                     // 线性降采样交点到OaOb向量上
//                     vertex.pos = Oa + k * (Ob - Oa);

//                     // 重置端点
//                     vertex.sptr_endPointa.reset();
//                     vertex.sptr_endPointb.reset();
//                 }
//             }
//         }
//     }
// }

float job0(const Vertex &vertex)
{
    QVector3D edgeVec = vertex.sptr_endPointb->pos - vertex.sptr_endPointa->pos;
    float edgeLengthSquared = edgeVec.lengthSquared();

    if (edgeLengthSquared < vertexEpsilon * vertexEpsilon) {
        return 0.5f; // 端点重合，取中点
    }

    // 计算交点到端点A的投影长度比例
    QVector3D toIntersection = vertex.pos - vertex.sptr_endPointa->pos;
    float t = QVector3D::dotProduct(toIntersection, edgeVec) / edgeLengthSquared;

    // 限制在 [0,1] 范围内
    return qBound(0.0f, t, 1.0f);
}

QVector3D projectVertexToCylinder(const QVector3D &vertex, const QVector4D &rgba, const QVector3D &centroid, const QVector3D &planeNormal, float radius, const QVector3D &vertexNormal)
{
    // 计算顶点相对于质心的向量
    QVector3D relativePos = vertex - QVector3D(centroid.x(), vertex.y(), centroid.z());

    float originalDistance = relativePos.length();

    if (originalDistance < vertexEpsilon) {
        return vertex; // 避免除以零
    }

    float t = qAbs(QVector3D::dotProduct(vertexNormal.normalized(), planeNormal.normalized()));
    float gt = QVector4D::dotProduct(rgba, {0.299f, 0.587f, 0.114f, 0.0f})/* * std::max(rgba.x(), std::max(rgba.y(), rgba.z()))*/;
    QVector3D projectedPos = vertex + relativePos.normalized() * (exp(t + gt) - 1) * radius;

    return projectedPos;
}

void job3(QVector<QVector<QVector<Vertex> > > &contours, const QVector3D &plane_norm, QVector<float>& out_slices_intervals, const QVector3D& u_axis, const QVector3D& v_axis,
    float dot_merge_threshold)// 合并阈值：dot值差异小于此值的点归为同一切片（避免精度导致的冗余切片）
{
    // 步骤1：收集job2产出的所有点（全量保留，不丢任何一个）
    QVector<QPair<float, Vertex>> all_points_with_dot;  // <点沿plane_norm的投影值, 顶点>
    for (auto& sliceContours : contours) {
        for (auto& contour : sliceContours) {
            for (auto& vertex : contour) {
                // 计算每个点沿plane_norm的投影值（dot值）
                float dot_val = QVector3D::dotProduct(vertex.pos, plane_norm);
                all_points_with_dot.append({dot_val, vertex});
            }
        }
    }

    if (all_points_with_dot.isEmpty()) {
        out_slices_intervals.clear();  // 空输入时清空输出
        return;
    }

    // 步骤2：按dot值排序（方便分组）
    std::sort(all_points_with_dot.begin(), all_points_with_dot.end(),
              [](const QPair<float, Vertex>& a, const QPair<float, Vertex>& b) {
                  return a.first < b.first;
              });

    // 步骤3：按dot值分组（合并接近值），每个组对应一个切片（全量包含该组所有点）
    QVector<float> slice_dot_values;  // 每个切片的代表性dot值（组内平均值，用于计算层间距）
    QVector<QVector<QVector<Vertex>>> full_contours;  // 最终切片（全量点）

    // 初始化第一个切片组
    QVector<Vertex> current_slice_points;
    float current_group_dot = all_points_with_dot[0].first;
    current_slice_points.append(all_points_with_dot[0].second);

    // 遍历所有点，按dot值分组（全量分配，不丢弃）
    for (size_t i = 1; i < all_points_with_dot.size(); ++i) {
        float dot_val = all_points_with_dot[i].first;
        Vertex vertex = all_points_with_dot[i].second;

        // 若当前点的dot值与组内dot值差异小于阈值，加入当前组；否则新建组
        if (qAbs(dot_val - current_group_dot) <= dot_merge_threshold) {
            current_slice_points.append(vertex);
            // 更新组的代表性dot值（平均值，使组内dot更集中）
            current_group_dot = (current_group_dot * current_slice_points.size() + dot_val) / (current_slice_points.size() + 1);
        } else {
            // 当前组满，生成切片（按原轮廓逻辑分组，保留点的关联性）
            QVector<QVector<Vertex>> slice_contours;
            // 按U-Z坐标聚类（保持轮廓连续性，不改变点的顺序和关联性）
            std::sort(current_slice_points.begin(), current_slice_points.end(),
                      [&u_axis, &v_axis](const Vertex& a, const Vertex& b) {
                          // 计算点在u_axis方向的投影（点积）
                          float proj_u_a = QVector3D::dotProduct(a.pos, u_axis);
                          float proj_u_b = QVector3D::dotProduct(b.pos, u_axis);
                          // 若u方向投影差异显著，按u排序
                          if (qAbs(proj_u_a - proj_u_b) > 1e-6f) {
                              return proj_u_a < proj_u_b;
                          }
                          // 否则按v_axis方向投影排序
                          float proj_v_a = QVector3D::dotProduct(a.pos, v_axis);
                          float proj_v_b = QVector3D::dotProduct(b.pos, v_axis);
                          return proj_v_a < proj_v_b;
                      });

            // 按相邻点距离聚类为轮廓（避免单个切片内点无序）
            QVector<Vertex> current_contour;
            if (!current_slice_points.isEmpty()) {
                current_contour.append(current_slice_points[0]);
                for (size_t j = 1; j < current_slice_points.size(); ++j) {
                    // 相邻点距离小于阈值，视为同一轮廓
                    float dist = current_slice_points[j].pos.distanceToPoint(current_slice_points[j-1].pos);
                    if (dist < 2e-3f) {  // 距离阈值可根据模型尺寸调整
                        current_contour.append(current_slice_points[j]);
                    } else {
                        slice_contours.append(current_contour);
                        current_contour.clear();
                        current_contour.append(current_slice_points[j]);
                    }
                }
                if (!current_contour.isEmpty()) {
                    slice_contours.append(current_contour);
                }
            }

            // 加入切片列表，并记录当前组的代表性dot值
            full_contours.append(slice_contours);
            slice_dot_values.append(current_group_dot);  // 保存当前切片的平均dot值

            // 初始化新组
            current_slice_points.clear();
            current_slice_points.append(vertex);
            current_group_dot = dot_val;
        }
    }

    // 处理最后一组点
    if (!current_slice_points.isEmpty()) {
        QVector<QVector<Vertex>> slice_contours;
        std::sort(current_slice_points.begin(), current_slice_points.end(),
                  [](const Vertex& a, const Vertex& b) {
                      if (qAbs(a.pos.y() - b.pos.y()) > 1e-6f) {
                          return a.pos.y() < b.pos.y();
                      }
                      return a.pos.z() < b.pos.z();
                  });

        QVector<Vertex> current_contour;
        current_contour.append(current_slice_points[0]);
        for (size_t j = 1; j < current_slice_points.size(); ++j) {
            float dist = current_slice_points[j].pos.distanceToPoint(current_slice_points[j-1].pos);
            if (dist < 2e-3f) {
                current_contour.append(current_slice_points[j]);
            } else {
                slice_contours.append(current_contour);
                current_contour.clear();
                current_contour.append(current_slice_points[j]);
            }
        }
        if (!current_contour.isEmpty()) {
            slice_contours.append(current_contour);
        }
        full_contours.append(slice_contours);
        slice_dot_values.append(current_group_dot);  // 保存最后一个切片的平均dot值
    }

    // 替换为全量结构化切片（无丢点、不等间距）
    contours = std::move(full_contours);

    // 计算并填充层间距（N个切片对应N-1个间距）
    out_slices_intervals.clear();  // 清空输出参数，避免残留数据
    if (slice_dot_values.size() >= 2) {
        for (int i = 1; i < slice_dot_values.size(); ++i) {
            // 相邻切片的dot值差即为沿法向量方向的层间距（假设plane_norm是单位向量）
            float interval = slice_dot_values[i] - slice_dot_values[i-1];
            out_slices_intervals.append(interval);
        }
    }
}
